﻿using System;
using System.Collections.Generic;

namespace D3.Pathing
{


    public static class DouglasPeucker
    {
        /// <summary>
        /// Uses the Douglas peucker alghorithm to reduce hops in a generated path.
        /// </summary>
        /// <param name="points">The points.</param>
        /// <param name="tolerance">The tolerance.</param>
        /// <returns></returns>
        /// <remarks>Created 2012-04-01</remarks>
        public static List<SpeedyAStarNode> DouglasPeuckerReduction(List<SpeedyAStarNode> points, double tolerance)
        {
            if (points == null || points.Count < 3)
                return points;

            int firstPoint = 0;
            int lastPoint = points.Count - 1;

            //Add the first and last index to the keepers
            List<int> pointIndexsToKeep = new List<int> {firstPoint, lastPoint};

            //The first and the last point cannot be the same
            while (points[firstPoint].Equals(points[lastPoint]))
            {
                lastPoint--;
            }

            DouglasPeuckerReduction(points, firstPoint, lastPoint, tolerance, ref pointIndexsToKeep);

            List<SpeedyAStarNode> returnPoints = new List<SpeedyAStarNode>();
            pointIndexsToKeep.Sort();

            foreach (int index in pointIndexsToKeep)
            {
                returnPoints.Add(points[index]);
            }

            return returnPoints;
        }


        private static int _tolerance = 1;

        public static void SetTolerance(int t )
        {
            _tolerance = t;
        }

        public static List<SpeedyAStarNode> DouglasPeuckerReduction(List<SpeedyAStarNode> points)
        {
            return DouglasPeuckerReduction(points, _tolerance);
        }

        public static void DouglasPeuckerReduction(List<SpeedyAStarNode> points, int firstPoint, int lastPoint, double tolerance,
                                                   ref List<int> pointIndexsToKeep)
        {
            double maxDistance = 0;
            int indexFarthest = 0;

            for (int index = firstPoint; index < lastPoint; index++)
            {
                double distance = PerpendicularDistance
                    (points[firstPoint], points[lastPoint], points[index]);
                if (distance > maxDistance)
                {
                    maxDistance = distance;
                    indexFarthest = index;
                }
            }

            if (maxDistance > tolerance && indexFarthest != 0)
            {
                //Add the largest point that exceeds the tolerance
                pointIndexsToKeep.Add(indexFarthest);

                DouglasPeuckerReduction(points, firstPoint, indexFarthest, tolerance, ref pointIndexsToKeep);
                DouglasPeuckerReduction(points, indexFarthest, lastPoint, tolerance, ref pointIndexsToKeep);
            }
        }

        /// <summary>
        /// Get the Perpendiculars distance.
        /// </summary>
        /// <param name="point1">The point1.</param>
        /// <param name="point2">The point2.</param>
        /// <param name="point">The point.</param>
        /// <returns></returns>
        /// <remarks>Created 2012-04-01</remarks>
        public static double PerpendicularDistance(SpeedyAStarNode point1, SpeedyAStarNode point2, SpeedyAStarNode point)
        {
            double area = Math.Abs(.5*(point1.X*point2.Y + point2.X*
                                                           point.Y + point.X*point1.Y - point2.X*point1.Y - point.X*
                                                                                                            point2.Y -
                                       point1.X*point.Y));
            double bottom = Math.Sqrt(Math.Pow(point1.X - point2.X, 2) +
                                      Math.Pow(point1.Y - point2.Y, 2));
            double height = area/bottom*2;

            return height;
        }
    }
}